#include <cstdio>
#include <time.h>
#include <string>
#include <iostream>
#include <fstream>

#include "cubex.h"
#include "main.h"
#include "cube.h"
#include "rubik.h"
#include "colorpad.h"

using namespace std;


//Ye

// shared variables...
const char* guiver = "1.30";
const int N = Cubex::N;
Cubex thecube;
string thesolution = "";
bool randm = false;
int inmode = 0;
string app = "cubex";
string cmd = ""; int cmdc;

// send a protocol-compliant solution to the receiver program.
void SendSolution() {
	if (!thecube.cubeinit) return;
	int MOV = thecube.MOV, mov[thecube.MOV+1], m = 0, mv;
	string a = ""; string cm = "";
	for (int i = 0; i <= MOV; i++) mov[i] = thecube.mov[i];
	if (thecube.cenfix) mv = MOV; else mv = MOV - 1;
	if (randm) {
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(j,2,-i)+48;
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(-2,-i,-j)+48;
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(j,-i,-2)+48;
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(2,-i,j)+48;
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(-j,-i,2)+48;
		for (int i = -1; i <= 1; i++)
			for (int j = -1; j <= 1; j++)
				cm += *thecube.face(j,-2,i)+48;
		if (thecube.cenfix) {
			cm += *thecube.face(0,1,0)+48;
			cm += *thecube.face(-1,0,0)+48;
			cm += *thecube.face(0,0,-1)+48;
			cm += *thecube.face(1,0,0)+48;
			cm += *thecube.face(0,0,1)+48;
			cm += *thecube.face(0,-1,0)+48;
		}
	}
	/*
	printf("200 cube solved ok.\n");
	printf("101 version %s%s by %s\n", guiver, Cubex::ver, Cubex::author);
	printf("202 %i moves %i groups", mov[0], mv);
	for (int i = 1; i <= mv; i++)
		printf(" %i", mov[i]);
	printf("\n");
	printf("220 starting diagram:\n");
	*/
	thecube.RenderScreen();
	/*
	printf("221 diagram end.\n");
	printf("210 sending solution:\n");
	*/
	ofstream solutionfile;
	solutionfile.open("solution.txt");

	for (int i = 1; i <= mv; i++) {
		for (int j = 1; j <= mov[i]; j++) {
			m++;
			a = thesolution.substr(m * 3 - 3, 3);
			if      (a == "UL.") { thecube.UL(); a = "UL"; } // top left
			else if (a == "UR.") { thecube.UR(); a = "UR"; } // top right
			else if (a == "DL.") { thecube.DL(); a = "DL"; } // bottom left
			else if (a == "DR.") { thecube.DR(); a = "DR"; } // bottom right
			else if (a == "LU.") { thecube.LU(); a = "LU"; } // left up
			else if (a == "LD.") { thecube.LD(); a = "LD"; } // left down
			else if (a == "RU.") { thecube.RU(); a = "RU"; } // right up
			else if (a == "RD.") { thecube.RD(); a = "RD"; } // right down
			else if (a == "FC.") { thecube.FC(); a = "FC"; } // front clockwise
			else if (a == "FA.") { thecube.FA(); a = "FA"; } // front counterclockwise
			else if (a == "BC.") { thecube.BC(); a = "BC"; } // back clockwise
			else if (a == "BA.") { thecube.BA(); a = "BA"; } // back counterclockwise
			else if (a == "ML.") { thecube.CL(); a = "CL"; } // middle left
			else if (a == "MR.") { thecube.CR(); a = "CR"; } // middle right
			else if (a == "MU.") { thecube.CU(); a = "CU"; } // middle up
			else if (a == "MD.") { thecube.CD(); a = "CD"; } // middle down
			else if (a == "MC.") { thecube.CC(); a = "CC"; } // middle clockwise
			else if (a == "MA.") { thecube.CA(); a = "CA"; } // middle counterclockwise
			else if (a == "CL.") { thecube.CL(); a = "CL"; } // whole cube left
			else if (a == "CR.") { thecube.CR(); a = "CR"; } // whole cube right
			else if (a == "CU.") { thecube.CU(); a = "CU"; } // whole cube up
			else if (a == "CD.") { thecube.CD(); a = "CD"; } // whole cube down
			else if (a == "CC.") { thecube.CC(); a = "CC"; } // whole cube clockwise
			else if (a == "CA.") { thecube.CA(); a = "CA"; } // whole cube counterclockwise
		//	printf("%s, ", a.c_str());
			solutionfile << a.c_str() << "\n";
		}
	}
	solutionfile.close();
	printf("\n");
	/*
	printf("211 completed solution.\n");
	//  printf("220 ending diagram:\n");
	//  thecube.RenderScreen();
	//  printf("221 diagram end.\n");
	if (randm)
		printf("203 cmd: %s %s\n", app.c_str(), cm.c_str());
	printf("201 terminating successfully.\n");
	*/
}
// take the command-line parameter and make a cube out of it
int ScrambleCubeGet() {
	string cm = "";
	srand(time(NULL));
	if (cmdc < 2) { return 1; }
	cm = cmd;
	thecube.ResetCube();
	if (cm == "random") {
		thecube.ScrambleCube();
		randm = true;
		return 0;
	}
	else if (cm == "random-centers") {
		thecube.ScrambleCube();
		thecube.cenfix = 1;
		randm = true;
		return 0;
	}
	if ((int)cm.find(" ") >= 0)
	{
		// NEW routine to deal with optional "U: D: L: R: F: B: C: " command lines
		int u,d,l,r,f,b,c,max=0,min=0;
		u = cm.find("U:"); if (u < 0) u = cm.find("u:");
		d = cm.find("D:"); if (d < 0) d = cm.find("d:");
		l = cm.find("L:"); if (l < 0) l = cm.find("l:");
		r = cm.find("R:"); if (r < 0) r = cm.find("r:");
		f = cm.find("F:"); if (f < 0) f = cm.find("f:");
		b = cm.find("B:"); if (b < 0) b = cm.find("b:");
		c = cm.find("C:"); if (c < 0) c = cm.find("c:");
		if (u < min) min = u; if (u > max) max = u;
		if (d < min) min = d; if (d > max) max = d;
		if (l < min) min = l; if (l > max) max = l;
		if (r < min) min = r; if (r > max) max = r;
		if (f < min) min = f; if (f > max) max = f;
		if (b < min) min = b; if (b > max) max = b;
		if (min < 0 || max > cm.length() - N * N - 2 || c > cm.length() - 6 - 2)
			return 1;
		cm  = cmd.substr(u+2,N*N);
		cm += cmd.substr(l+2,N*N);
		cm += cmd.substr(f+2,N*N);
		cm += cmd.substr(r+2,N*N);
		cm += cmd.substr(b+2,N*N);
		cm += cmd.substr(d+2,N*N);
		if (c >= 0)
			cm += cmd.substr(c+2,6);
	}
	thecube.cubeinit = false;
	if (cm.length() < N*N*6) {
		return 1;
	}
	for (int i = 1; i <= N*N*6; i++) {
		if (cm.at(i-1)-48 < 1 || cm.at(i-1)-48 > 6) {
			return 1;
		}
	}
	for (int i = -1; i <= 1; i++) {
		for (int j = -1; j <= 1; j++) {
			*thecube.face(j, 2, -i) = cm.at(i*3+j+4)-48;
			*thecube.face(-2, -i, -j) = cm.at(i*3+j+13)-48;
			*thecube.face(j, -i, -2) = cm.at(i*3+j+22)-48;
			*thecube.face(2, -i, j) = cm.at(i*3+j+31)-48;
			*thecube.face(-j, -i, 2) = cm.at(i*3+j+40)-48;
			*thecube.face(j, -2, i) = cm.at(i*3+j+49)-48;
		}
	}
	thecube.cubeinit = true;
	cm = cm.substr(N*N*6, cm.length() - N*N*6);
	thecube.cenfix = 0;
	if (cm.length() >= 6) {
		thecube.cenfix = 1;
		for (int i = 1; i <= 6; i++) {
			if (cm.at(i-1)-48 < 0 || cm.at(i-1)-48 > 3)
				thecube.cenfix = 0;
		}
		if (thecube.cenfix == 1) {
			for (int i = 0; i <= 0; i++) {
				for (int j = 0; j <= 0; j++) {
					*thecube.face(j, 1, -i) = cm.at(i*1+j+0)-48;
					*thecube.face(-1, -i, -j) = cm.at(i*1+j+1)-48;
					*thecube.face(j, -i, -1) = cm.at(i*1+j+2)-48;
					*thecube.face(1, -i, j) = cm.at(i*1+j+3)-48;
					*thecube.face(-j, -i, 1) = cm.at(i*1+j+4)-48;
					*thecube.face(j, -1, i) = cm.at(i*1+j+5)-48;
				}
			}
			cm = cm.substr(6, cm.length() - 6);
		}
	}
	return 0;
}

int solve(string filename) {
	cmdc = 2;
	ifstream configfile(filename);
	string config;
	getline(configfile,config);

	string argv[2] = {"cub",config};
	if (cmdc >= 7) {
		inmode = 1;
		cmd = argv[1];
		for (int i = 2; i <= 6; i++) {
			cmd += " ";
			cmd += argv[i];
		}
		if (cmdc >= 8) {
			cmd += " ";
			cmd += argv[7];
		}
	}
	else if (cmdc >= 2) {
		inmode = 0;
		cmd = argv[1];
	}
	app = argv[0];
	int n;
	n = app.rfind("\\");
	if (n > 0)
		app = app.substr(n+1, app.length()-n-1);
	n = app.rfind("/");
	if (n > 0)
		app = app.substr(n+1, app.length()-n-1);
	int x = 0, y = 0;
	y = ScrambleCubeGet();
	if (y == 0) {
		x = thecube.SolveCube();
		if (x == 0) {
			thesolution = thecube.solution;
			SendSolution();
		}
		else {
			printf("500 ERROR: solver failed for the following reason:\n");
			if      (x == 1)
				printf("511 ERROR: cubelet error - incorrect cubelets - cube mispainted.\n");
			else if (x == 2)
				printf("512 ERROR: parity error - nondescript - cube misassembled.\n");
			else if (x == 3)
				printf("513 ERROR: parity error - center rotation - cube misassembled.\n");
			else if (x == 4)
				printf("514 ERROR: cubelet error - backward centers or corners - cube mispainted.\n");
			else if (x == 5)
				printf("515 ERROR: parity error - edge flipping - cube misassembled.\n");
			else if (x == 6)
				printf("516 ERROR: parity error - edge swapping - cube misassembled.\n");
			else if (x == 7)
				printf("517 ERROR: parity error - corner rotation - cube misassembled.\n");
			printf("220 your diagram:\n");
			thecube.RenderScreen();
			printf("221 diagram end.\n");
			printf("501 terminating unsuccessfully.\n");
		}
	}
	else {
		printf("500 ERROR: solver failed for the following reason:\n");
		printf("510 ERROR: non-protocol input entered.\n");
		printf(
			"\
			400 syntax:\n\
			%s <cube-layout>[<center-rotations>]\n\
			%s random[-centers]\n\
			example command-lines:\n\
			cubex 212212212333333333626626626555555555141141141464464464\n\
			cubex 111111111222333222555222555444555444333444333666666666100003\n\
			cubex random\n\
			notes:\n\
			- hint: try using the random parameter to get a feel for how to input cubes.\n\
			- read readme.txt (included with this distribution) for protocol details.\n\
			- use a terminal with scrollback feature to use this program! (or pipe it)\n\
			401 end of notes.\n\
			",
			app.c_str(), app.c_str()
			);
		printf("501 terminating unsuccessfully.\n");
	}
	if (x != 0 || y != 0) return -1;
	return 0;
}
// end Ye

// viewport
struct Viewport {
	Viewport(): mousePos(0.0,0.0) { orientation = identity3D(); };
	int w, h; // width and height
	vec2 mousePos;
	mat4 orientation;
};

//****************************************************
// Global Variables
//****************************************************
Viewport viewport;
UCB::ImageSaver * imgSaver;
int frameCount = 0;
rubik *r;
colorpad *cp;
bool test, manipulate=false, rot_flag=false, toggle=false, autoSolve=false, solo=false;
rotate_d dir=STAY;
double degrees=0;
bool solving = true;
vec3 *cur_color = NULL;
RUBIK_SIDES current=NONE;
RUBIK_ROTATE curdir =NONE_R;

mat4 rubikAngle;
mat4 normalAngle;
enum BACKWARD_STAGE {ROTATE_LEFT,TWIST,ROTATE_RIGHT, DO_NOTHING};

BACKWARD_STAGE back=DO_NOTHING;
// A simple helper function to load a mat4 into opengl
void applyMat4(mat4 &m) {
	double glmat[16];
	int idx = 0;
	for (int j = 0; j < 4; j++) 
		for (int i = 0; i < 4; i++)
			glmat[idx++] = m[i][j];
	glMultMatrixd(glmat);
}

// setup the model view matrix for mesh rendering
void setupView() {
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0,0,-3);
	applyMat4(viewport.orientation);
}

void write(double x, double y, string text) {

	glRasterPos3f(x,y,0);
	for (int i=0; i<text.size();i++) {
		glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, (int) text[i]);
	}
}

//-------------------------------------------------------------------------------
/// You will be calling all of your drawing-related code from this function.
/// Nowhere else in your code should you use glBegin(...) and glEnd() except code
/// called from this method.
///
/// To force a redraw of the screen (eg. after mouse events or the like) simply call
/// glutPostRedisplay();


// Simple testing function

void draw9Cubes(double c) {

	cube(vec3(-0.3,-0.3,c)).drawCube();
	cube(vec3(0,-0.3,c)).drawCube();
	cube(vec3(0.3,-0.3,c)).drawCube();

	cube(vec3(-0.3,0,c)).drawCube();
	cube(vec3(0,0,c)).drawCube();
	cube(vec3(0.3,0,c)).drawCube();

	cube(vec3(-0.3,0.3,c)).drawCube();
	cube(vec3(0,0.3,c)).drawCube();
	cube(vec3(0.3,0.3,c)).drawCube();

}

void rotateFrame () {
	if (dir != STAY) {
		switch (dir) {

		case UP_D:
			if (degrees < 90) {
				degrees +=3;
				r->setX(3);
				cout << degrees << endl;
			} else {
				degrees=0;
				dir=STAY;
				r->test();
				r->rubikRotateX(CW);
				r->test();
				r->resetMyAll();
				r->checkXYZ(); 

			}

			break;
		case DOWN_D:
			if (degrees < 90) {
				degrees +=3;
				r->setX(-3);					
			} else {
				degrees=0;
				dir=STAY;
				r->test();
				r->rubikRotateX(CCW);
				r->test();
				r->resetMyAll();
				r->checkXYZ(); 
			}

			break;
		case RIGHT_D:
			if (degrees < 90) {
				degrees +=3;
				r->setY(-3);
			} else {
				degrees=0;
				dir=STAY;
				cout << "BFORE :" << endl;
				r->test();
				r->rubikRotateY(CW);
				r->test();

				if (back==ROTATE_RIGHT) {
					back=DO_NOTHING;
				}
				r->resetMyAll();
				r->checkXYZ(); 
			}

			break;
		case LEFT_D:
			if (degrees < 90) {
				degrees +=3;
				r->setY(3);
			} else {

				if (back == ROTATE_LEFT) {
					back = TWIST;
				}

				degrees=0;
				dir=STAY;
				r->test();
				r->rubikRotateY(CCW);
				r->test();
				r->resetMyAll();
				r->checkXYZ(); 
			}
			break;
		case CCW_D:
			if (degrees < 90) {
				degrees+=3;
				r->setZ(3);
			} else {
				degrees=0;
				dir=STAY;
			}
			break;
		case CW_D:
			if (degrees > -90) {
				degrees -=3;
				r->setZ(-3);
			} else {
				degrees =0;
				dir=STAY;
			}
			break;
		}
	}
}

void parseSoln(string s) {
	char dir=s[1];
	char face = s[0];
	if (current == NONE) {
	switch (face) {
	case 'U':

		switch (dir) {
		case 'L':
			current = TOP;
			curdir = CW;
			manipulate=true;
			break;

		case 'R':
			current = TOP;
			curdir=CCW;
			manipulate=true;
			break;
		}

		break;
	case 'D':
		switch (dir) {
		case 'L':
			current = BOTTOM;
			curdir = CCW;
			manipulate=true;
			break;
		case 'R':
			current = BOTTOM;
			curdir=CW;
			manipulate=true;
			break;
		}

		break;
	case 'F':
		switch (dir) {
		case 'A':
			current = FORWARD;
			curdir = CCW;
			manipulate=true;
			break;
		case 'C':
			current = FORWARD;
			curdir=CW;
			manipulate=true;
			break;
		}
		break;
	case 'B':
		switch (dir) {
		case 'C':
			current = BACKWARD;
			curdir = CW;
			manipulate=true;
			break;
		case 'A':
			current = BACKWARD;
			curdir=CCW;
			manipulate=true;
			break;
		}
		break;
	case 'L':
		switch (dir) {
		case 'U':
			current = LEFT;
			curdir = CCW;
			manipulate=true;
			break;
		case 'D':
			current = LEFT;
			curdir = CW;
			manipulate=true;
			break;
		}
		break;
	case 'R':
		switch (dir) {
		case 'U':
			current = RIGHT;
			curdir = CW;
			manipulate=true;
			break;
		case 'D':
			current = RIGHT;
			curdir = CCW;
			manipulate=true;
			break;
		}
		break;
	}
	}
}

void myTwistFunc () {
	if (manipulate) {
		switch(current) {

		case LEFT:
			switch (curdir) {
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateLX(CCW);
			} else {
				degrees=0;
				manipulate=false;
				if (back == TWIST) {
				rot_flag=true;
				dir=RIGHT_D;
				back=ROTATE_RIGHT;
				}
				current=NONE;
				r->test();
				r->rotateMyFaces(LEFT, CW);
				r->test();
				r->resetAll();
			}
			break;
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateLX(CW);
			} else {
				degrees=0;
				manipulate=false;
				if (back == TWIST) {
				rot_flag=true;
				dir=RIGHT_D;
				back=ROTATE_RIGHT;
				}
				current=NONE;
				r->test();
				r->rotateMyFaces(LEFT, CCW);
				r->test();
				r->resetAll();
			}
			break;
			}

			break;
		case RIGHT:
			switch (curdir) {
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateRX(CW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(RIGHT, CW);
				r->test();
				r->resetAll();
			}
			break;
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateRX(CCW);
			} else {
				degrees=0;

				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(RIGHT, CCW);
				r->test();
				r->resetAll();
			}
			break;
			}
			break;
		case TOP:
			switch (curdir) {
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateTY(CW);
			} else {
				degrees=0;
				manipulate=false;

				current=NONE;				r->test();
				r->rotateMyFaces(TOP, CW);
				r->test();
				r->resetAll();
			}
				break;
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateTY(CCW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(TOP, CCW);
				r->test();
				r->resetAll();
			}
			break;
			}
		break;
		case BOTTOM:
			switch (curdir) {
			case CW:
				if (degrees < 90) {
				degrees +=3;
				r->rotateLY(CW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(BOTTOM, CW);
				r->test();
				r->resetAll();
			}

			break;
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateLY(CCW);
				cout << degrees << endl;
			} else {
				degrees=0;

				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(BOTTOM, CCW);
				r->test();
				r->resetAll();
			}

			break;
			}
			break;
		case MIDDLE_Y:
			switch (curdir) {
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateMY(CCW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(MIDDLE_Y, CCW);
				r->test();
				r->resetAll();
			}
			break;
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateMY(CW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(MIDDLE_Y, CW);
				r->test();
				r->resetAll();
			}
				break;
			}
		break;
		case MIDDLE_X:
			switch (curdir) {
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateMX(CW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(MIDDLE_X, CCW);
				r->test();
				r->resetAll();
			}
			break;
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateMX(CCW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(MIDDLE_X, CW);
				r->test();
				r->resetAll();
			}
				break;
			}
		break;
		case FORWARD:
			switch (curdir) {
			case CW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateRZ(CW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(FORWARD, CW);
				r->test();
				r->resetAll();
			}
			break;
			case CCW:
			if (degrees < 90) {
				degrees +=3;
				r->rotateRZ(CCW);
			} else {
				degrees=0;
				manipulate=false;
				current=NONE;
				r->test();
				r->rotateMyFaces(FORWARD, CCW);
				r->test();
				r->resetAll();
			}
				break;
			}
			break;
		case BACKWARD:
				back=ROTATE_LEFT;
				dir=LEFT_D;
				current=LEFT;
				rot_flag=true;
			switch (curdir) {

			case CW:
				curdir=CCW;
				break;
			case CCW:
				curdir=CW;
				break;
			}
			break;
		}
	}
}



void
print_stroke_string(void* font, char* s)
{
   if (s && strlen(s)) {
      while (*s) {
         glutStrokeCharacter(font, *s);
         s++;
      }
   }
}

void
print_bitmap_string(void* font, char* s)
{
   if (s && strlen(s)) {
      while (*s) {
         glutBitmapCharacter(font, *s);
         s++;
      }
   }
}

void 
my_display(void)
{

   /* Clear the window. */
   glClearColor(0.0, 0.0, 0.0, 0.0);
   glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
   glColor4f(1,1,1,0);
   
   print_bitmap_string(GLUT_BITMAP_TIMES_ROMAN_10, "HIIIIIIIII");
   
   glutSwapBuffers();
}

void display() {

	//Clear Buffers
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	setupView();

	
	if (solving){
		viewport.orientation=rubikAngle;
		if (rot_flag){
			rotateFrame();
			/*r->render();
			cout << "Saving BMPS" << endl;
			imgSaver->saveFrame();
			*/
		}
		if (autoSolve && (current==NONE) && (back==DO_NOTHING)) {
				string s;
				if ((s=r->nextSolution()) != "") {
				cout << s << endl;
				parseSoln(s);
				}
				if (solo) autoSolve=false;
		}

		if (manipulate && (back==DO_NOTHING || back==TWIST)) {
			myTwistFunc();	
		}
		
		r->render();

	} else {
		//taking input
		//use s to toggle solving
		viewport.orientation=normalAngle;
		r->renderflat();
		cp->render();

		
	}

	//Now that we've drawn on the buffer, swap the drawing buffer and the displaying buffer.
	glutSwapBuffers();



}


//-------------------------------------------------------------------------------
/// \brief	Called when the screen gets resized.
/// This gives you the opportunity to set up all the relevant transforms.
///
void reshape(int w, int h) {
	glViewport(0, 0, w, h);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45.0, ((double)w / MAX(h, 1)), 1.0, 100.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

//-------------------------------------------------------------------------------
/// Called whenever the mouse moves without any buttons pressed.
void myPassiveMotionFunc(int x, int y) {

	viewport.mousePos = vec2((double)x / glutGet(GLUT_WINDOW_WIDTH),(double)y / glutGet(GLUT_WINDOW_HEIGHT));

	//Force a redraw of the window.
	glutPostRedisplay();
}

//-------------------------------------------------------------------------------
/// Called whenever the mouse moves while a button is pressed
void myActiveMotionFunc(int x, int y) {
	// Rotate viewport orientation proportional to mouse motion
	if (solving){
		vec2 newMouse = vec2((double)x / glutGet(GLUT_WINDOW_WIDTH),(double)y / glutGet(GLUT_WINDOW_HEIGHT));
		vec2 diff = (newMouse - viewport.mousePos);
		double len = diff.length();
		if (len > .001) {
			vec3 axis = vec3(diff[1]/len,diff[0]/len, 0);
			viewport.orientation = rotation3D(axis, 180*len) * viewport.orientation;
		}

		//Record the mouse location for drawing crosshairs
		viewport.mousePos = newMouse;
	} 
	//Force a redraw of the window.
	glutPostRedisplay();
}

void manipulateCube() {

}

rotate_d getDir (RUBIK_ORIENTATION o, rotate_d d) {
	switch(o.frontSide) {
	case FORWARD:
		switch (o.topSide) {
			//DONE
		case TOP:
			switch (d) {
				//DONE
			case LEFT_D:
				return LEFT_D;
				//DONE
			case RIGHT_D:
				return RIGHT_D;
				//DONE
			case UP_D:
				return UP_D;
				break;
				//DONE
			case DOWN_D:
				return DOWN_D;
				break;
			}

			break;
			
		case BOTTOM:
			
			switch (d) {

			case LEFT_D:
				return RIGHT_D;
				break;
			case RIGHT_D:
				return LEFT_D;
				break;
			case UP_D:
				return DOWN_D;
				break;
			case DOWN_D:
				return UP_D;
				break;
			}

			break;
		case LEFT:
			switch (d) {

			case LEFT_D:
				return DOWN_D;
				break;
			case RIGHT_D:
				return UP_D;
				break;
			case UP_D:
				return LEFT_D;
				break;
			case DOWN_D:
				return RIGHT_D;
				break;
			}
			break;
		case RIGHT:
			switch (d) {

			case LEFT_D:
				return UP_D;
				break;
			case RIGHT_D:
				return DOWN_D;
				break;
			case UP_D:
				return RIGHT_D;
				break;
			case DOWN_D:
				return LEFT_D;
				break;
			}
			break;
		}
		break;
	case BACKWARD:
		switch (o.topSide) {
		//DONE
		case TOP:
			switch (d) {
			case LEFT_D:
				return LEFT_D;
				break;
			case RIGHT_D:
				return RIGHT_D;
				break;
			case UP_D:
				return DOWN_D;
				break;
			case DOWN_D:
				return UP_D;				
				break;
			}

			break;
			//DONE
		case BOTTOM:
			
			switch (d) {

			case LEFT_D:
				return RIGHT_D;
				break;
			case RIGHT_D:
				return LEFT_D;
				break;
			case UP_D:
				return UP_D;
				break;
			case DOWN_D:
				return DOWN_D;
				break;
			}

			break;
		case LEFT:
			switch (d) {

			case LEFT_D:
				return DOWN_D;
				break;
			case RIGHT_D:
				return UP_D;
				break;
			case UP_D:
				return RIGHT_D;
				break;
			case DOWN_D:
				return LEFT_D;
				break;
			}
			break;
		case RIGHT:
			switch (d) {

			case LEFT_D:
				return UP_D;
				break;
			case RIGHT_D:
				return DOWN_D;
				break;
			case UP_D:
				return LEFT_D;
				break;
			case DOWN_D:
				return RIGHT_D;
				break;
			}
			break;
		}
		break;
	case LEFT:

		switch (o.topSide) {
		case TOP:

			switch (d) {

			case LEFT_D:
				return LEFT_D;
				break;
			case RIGHT_D:
				return RIGHT_D;
				break;
			case UP_D:
				return CCW_D;
				break;
			case DOWN_D:
				return CW_D;
				break;
			}

			break;
		case BOTTOM:
			
			switch (d) {

			case LEFT_D:
				return RIGHT_D;
				break;
			case RIGHT_D:
				return LEFT_D;
				break;
			case UP_D:
				return CW_D;
				break;
			case DOWN_D:
				return CCW_D;
				break;
			}

			break;
		case FORWARD:
			switch (d) {

			case LEFT_D:
				return CCW_D;
				break;
			case RIGHT_D:
				return CW_D;
				break;
			case UP_D:
				return RIGHT_D;
				break;
			case DOWN_D:
				return LEFT_D;
				break;
			}
			break;
		case BACKWARD:
			switch (d) {

			case LEFT_D:
				return CW_D;
				break;
			case RIGHT_D:
				return CCW_D;
				break;
			case UP_D:
				return LEFT_D;
				break;
			case DOWN_D:
				return RIGHT_D;
				break;
			}
			break;
		}

		break;
	case RIGHT:

		switch (o.topSide) {
		case TOP:

			switch (d) {

			case LEFT_D:
				return LEFT_D;
				break;
			case RIGHT_D:
				return RIGHT_D;
				break;
			case UP_D:
				return CW_D;
				break;
			case DOWN_D:
				return CCW_D;
				break;
			}

			break;
		case BOTTOM:
			
			switch (d) {

			case LEFT_D:
				return RIGHT_D;
				break;
			case RIGHT_D:
				return LEFT_D;
				break;
			case UP_D:
				return CCW_D;
				break;
			case DOWN_D:
				return CW_D;
				break;
			}

			break;
		case FORWARD:
			switch (d) {

			case LEFT_D:
				return CCW_D;
				break;
			case RIGHT_D:
				return CW_D;
				break;
			case UP_D:
				return LEFT_D;
				break;
			case DOWN_D:
				return RIGHT_D;
				break;
			}
			break;
		case BACKWARD:
			switch (d) {

			case LEFT_D:
				return CW_D;
				break;
			case RIGHT_D:
				return CCW_D;
				break;
			case UP_D:
				return RIGHT_D;
				break;
			case DOWN_D:
				return LEFT_D;
				break;
			}
			break;
		}
		break;
	case TOP:

		switch (o.topSide) {
		case BACKWARD:

			switch (d) {

			case LEFT_D:
				return CW_D;
				break;
			case RIGHT_D:
				return CCW_D;
				break;
			case UP_D:
				return UP_D;
				break;
			case DOWN_D:
				return DOWN_D;
				break;
			}

			break;
		case FORWARD:
			
			switch (d) {

			case LEFT_D:
				return CCW_D;
				break;
			case RIGHT_D:
				return CW_D;
				break;
			case UP_D:
				return DOWN_D;
				break;
			case DOWN_D:
				return UP_D;
				break;
			}

			break;
		case LEFT:
			switch (d) {

			case LEFT_D:
				return DOWN_D;
				break;
			case RIGHT_D:
				return UP_D;
				break;
			case UP_D:
				return CW_D;
				break;
			case DOWN_D:
				return CCW_D;
				break;
			}
			break;
		case RIGHT:
			switch (d) {

			case LEFT_D:
				return UP_D;
				break;
			case RIGHT_D:
				return DOWN_D;
				break;
			case UP_D:
				return CCW_D;
				break;
			case DOWN_D:
				return CW_D;
				break;
			}
			break;
		}

		break;
	case BOTTOM:

		switch (o.topSide) {
		case BACKWARD:

			switch (d) {

			case LEFT_D:
				return CW_D;
				break;
			case RIGHT_D:
				return CCW_D;
				break;
			case UP_D:
				return DOWN_D;
				break;
			case DOWN_D:
				return UP_D;
				break;
			}

			break;
		case FORWARD:
			
			switch (d) {

			case LEFT_D:
				return CCW_D;
				break;
			case RIGHT_D:
				return CW_D;
				break;
			case UP_D:
				return UP_D;
				break;
			case DOWN_D:
				return DOWN_D;
				break;
			}

			break;
		case LEFT:
			switch (d) {

			case LEFT_D:
				return DOWN_D;
				break;
			case RIGHT_D:
				return UP_D;
				break;
			case UP_D:
				return CCW_D;
				break;
			case DOWN_D:
				return CW_D;
				break;
			}
			break;
		case RIGHT:
			switch (d) {

			case LEFT_D:
				return UP_D;
				break;
			case RIGHT_D:
				return DOWN_D;
				break;
			case UP_D:
				return CW_D;
				break;
			case DOWN_D:
				return CCW_D;
				break;
			}
			break;
		}

		break;
	}
}

void myKeyboardSpecFunc (int key, int x, int y) {

	RUBIK_ORIENTATION ro = r->getOrientation();

	switch (key) {
	case GLUT_KEY_LEFT:
		if (rot_flag && (dir==STAY)) {
			dir=LEFT_D;//getDir(ro,LEFT_D);
			cout << "BEFORE : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
			//r->nextOrient(LEFT_D);
			cout << "AFTER : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
		}
		break;
	case GLUT_KEY_RIGHT:
		if (rot_flag && (dir==STAY)) {
			dir=RIGHT_D;//getDir(ro,RIGHT_D);
			cout << "BEFORE : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
			//r->nextOrient(RIGHT_D);
			cout << "AFTER : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
		}
		break;
	case GLUT_KEY_UP:
		if (rot_flag && (dir==STAY)) {
			dir=UP_D;//getDir(ro,UP_D);
			cout << "BEFORE : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
			//r->nextOrient(UP_D);
			cout << "AFTER : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
		}
		break;
	case GLUT_KEY_DOWN:
		if (rot_flag && (dir==STAY)) {
			dir=DOWN_D;//getDir(ro,DOWN_D);
			cout << "BEFORE : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
			//r->nextOrient(DOWN_D);
			cout << "AFTER : " << r->getOrientation().frontSide << " AND " << r->getOrientation().topSide << endl;
		}
		break;
	default:
		break;
	}
}

void myMouseFunc(int button, int state, int x, int y) {
	setupView();
	if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN && !solving ){
		//cout << "setting color" << endl;
		vec3 *tmp = cp->setColor(x,y);
	//	cout << endl;
		if (tmp != NULL) cur_color = tmp;

		if (cur_color != NULL){
			r->resetFaceColor(x,y,cur_color);
		}
	}
}



//-------------------------------------------------------------------------------
/// Called to handle keyboard events.
void myKeyboardFunc (unsigned char key, int x, int y) {
	int max_x=glutGet(GLUT_WINDOW_WIDTH);
	int max_y=glutGet(GLUT_WINDOW_HEIGHT);
	int _x,_y;
	switch (key) {
	case 'b':
		if (!manipulate) {

			if (!toggle) {
				current = BACKWARD;
				curdir=CW;
			} else {
				current=BACKWARD;
				curdir=CCW;
			}
			manipulate=true;
		}
		break;
	case '0':
		if (!manipulate) {

			if (!toggle) {
				current = FORWARD;
				curdir=CW;
			} else {
				current=FORWARD;
				curdir=CCW;
			}
			manipulate=true;
		}
		break;
	case '1':
		if (!manipulate) {

			if (!toggle) {
				current = LEFT;
				curdir=CW;
			} else {
				current=BOTTOM;
				curdir=CCW;
			}
			manipulate=true;
		}
		break;
	case '2':
		if (!manipulate) {
			curdir=CW;
			current=MIDDLE_X;
			manipulate=true;
		}
		break;
	case '3':
		if(!manipulate) {
			if (!toggle) {
				current = RIGHT;
				curdir=CCW;
			} else {
				current=BOTTOM;
				curdir=CW;
			}
			manipulate=true;
		}
		break;
	case '4':
		if (!manipulate) {
			curdir=CW;
			current=MIDDLE_Y;
			manipulate=true;
		}
		break;
	case '5':
		toggle=!toggle;
		break;
	case '6':
		if (!manipulate) {
			curdir=CCW;
			current=MIDDLE_Y;
			manipulate=true;
		}
		break;
	case '7':
		if (!manipulate) {
			if (!toggle) {
				current = LEFT;
				curdir=CCW;
			} else {
				current=TOP;
				curdir=CW;
			}
			manipulate=true;
		}
		break;
	case '8':
		if (!manipulate) {
			curdir=CCW;
			current=MIDDLE_X;
			manipulate=true;
		}
		break;
	case '9':
		if(!manipulate) {
			if (!toggle) {
				current = RIGHT;
				curdir=CW;
			} else {
				current=TOP;
				curdir=CCW;
			}
			manipulate=true;
		}
		break;
	case '27':			// Escape key
		exit(0);
		break;
	case 'a': //solve "again"
	case 'A':
		if (solving){
			r->writeConfiguration("./configuration.txt");
			int success = solve("./configuration.txt");
			if (success == 0){
				r->readSolution("./solution.txt");
				r->printSolutions();
			} else {
				cout << "Impossible to solve" << endl;
			}
		}
		break;
	case 'S':
	case 's':
		/*imgSaver->saveFrame();
		break;
		*/
		solving = !solving;
		if (solving){
			r->writeConfiguration("./configuration.txt");
			int success = solve("./configuration.txt");
			if (success == 0){
				r->readSolution("./solution.txt");
				r->printSolutions();
			} else {
				solving = !solving;
				cout << "Impossible to solve" << endl;
			}
		}
		break;
	case 'r':
		if (dir==STAY) {
			rot_flag=!rot_flag;
			cout << rot_flag << endl;
		}
		break;
	case 'g':
		autoSolve=!autoSolve;
		cout << "AUTOSOLVE: " << autoSolve << endl;
		break;
	case 'z':
		if (!manipulate && !autoSolve) solo=!solo;
		break;
	case 'Y':
	case 'y':
		cout << "Saving BMPS" << endl;

		for (_x=0,_y=0; _x < max_x && _y<max_y ; _x=_x+2,_y=_y+2){
			myActiveMotionFunc(x,y);
			display();
			imgSaver->saveFrame();
		}
		break;
	}
}

//-------------------------------------------------------------------------------
/// Called to update the screen at 30 fps.
void frameTimer(int value) {
	frameCount++;
	glutPostRedisplay();
	glutTimerFunc(1000/30, frameTimer, 1);
}



//-------------------------------------------------------------------------------
/// Initialize the environment
int main(int argc,char** argv) {
	//Initialize OpenGL

	glutInit(&argc,argv);
	glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);

	//Set up global variables
	viewport.w = 600;
	viewport.h = 600;

	//Initialize the screen capture class to save BMP captures
	//in the current directory, with the prefix "ik"
	imgSaver = new UCB::ImageSaver("./outputs", "output");
	r = new rubik();
	

	cp = new colorpad();
	vec3 axis = vec3(1,1,0);
	rubikAngle= rotation3D(axis, 22.5) * viewport.orientation;
	normalAngle=viewport.orientation;
	//Create OpenGL Window
	glutInitWindowSize(viewport.w,viewport.h);
	glutInitWindowPosition(0,0);
	glutCreateWindow("Rubik's Cube Solver - by Venketaram Ramachandran & Ye Yuan");

	//Register event handlers with OpenGL.
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(myKeyboardFunc);
	glutMotionFunc(myActiveMotionFunc);
	glutPassiveMotionFunc(myPassiveMotionFunc);
	glutMouseFunc(myMouseFunc);
	glutSpecialFunc(myKeyboardSpecFunc);
	frameTimer(0);

	glClearColor(0.1,0.1,.2,0);

	// set some lights
	{
		float ambient[3] = { .5f, .5f, .5f };
		float diffuse[3] = { .5f, .5f, .5f };
		float spot[4] = { 0, 0, 1, 1 };
		glLightfv(GL_LIGHT1, GL_AMBIENT, ambient);
		glLightfv(GL_LIGHT1, GL_DIFFUSE, diffuse);
		glLightfv(GL_LIGHT1, GL_POSITION, spot);
		glEnable(GL_LIGHT1);
	}
	{
		float ambient[3] = { .5f, .5f, .5f };
		float diffuse[3] = { .5f, .5f, .5f };
		float pos[4] = { 0, 0, 0, 1 };
		glLightfv(GL_LIGHT2, GL_AMBIENT, ambient);
		glLightfv(GL_LIGHT2, GL_DIFFUSE, diffuse);
		glLightfv(GL_LIGHT2, GL_POSITION, pos);
		glEnable(GL_LIGHT2);
	}
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_LIGHTING);
	glEnable(GL_DEPTH_TEST);
	glutMainLoop();
}
